#include "main.h"
#include "TOF.h"
#include "./BSP/ATK_MS53L0/atk_ms53l0.h"
#include "usart.h"
#include "./SYSTEM/delay/delay.h"
#include "Lowpass_Filter.h"
#define DEMO_DEV_IIC_ADDDR  (ATK_MS53L0_IIC_ADDR + 1)               /* IIC通信地址 */
#define DEMO_DEVICE_MODE    VL53L0X_DEVICEMODE_CONTINUOUS_RANGING   /* 设备工作模式：连续测距 */
#define DEMO_BUDGET_TIME    (20*1000)                               /* 测量预算时间(微秒) */
struct LOWPASS TOF_Filter = {0.01};   /* TOF数据低通滤波器结构体初始化，滤波系数0.01 */
/* VL53L0X设备结构体 */
static VL53L0X_Dev_t demo_dev = {
    .I2cDevAddr = ATK_MS53L0_IIC_ADDR,                              /* 从机默认IIC通信地址 */
};

/**
 * @brief       检测设备并配置IIC地址
 * @param       dev     : 设备结构体指针
 *              iic_addr: 设备IIC通信地址
 * @retval      无
 * @note        如果设备ID检测失败，会进入死循环
 */
static void demo_detect_device(VL53L0X_DEV dev, uint8_t iic_addr)
{
    uint16_t module_id = 0;
    
    /* 读取设备模块ID */
    VL53L0X_RdWord(dev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &module_id);
    if (module_id != ATK_MS53L0_MODULE_ID)
    {
        /* 模块ID不匹配，进入死循环等待 */
        while (1)
        {
            /* 此处可添加错误提示或LED闪烁等处理 */
            delay_ms(200);
        }
    }
    
    /* 配置设备IIC通信地址 */
    if (iic_addr != dev->I2cDevAddr)
    {
        VL53L0X_SetDeviceAddress(dev, iic_addr << 1);
        dev->I2cDevAddr = iic_addr;
    }
    
    /* 设备基本数据初始化 */
    VL53L0X_DataInit(dev);
}

/**
 * @brief       配置设备工作参数
 * @param       dev: 设备结构体指针
 * @retval      无
 */
static void demo_config_device(VL53L0X_DEV dev)
{
    uint8_t vhvsettings;
    uint8_t phasecal;
    uint32_t refspadcount;
    uint8_t isaperturespads;
    
    /* 设备静态初始化 */
    VL53L0X_StaticInit(dev);
    
    /* 执行参考校准 */
    VL53L0X_PerformRefCalibration(dev, &vhvsettings, &phasecal);
    
    /* 执行参考Spad管理 */
    VL53L0X_PerformRefSpadManagement(dev, &refspadcount, &isaperturespads);
    
    /* 设置设备工作模式为连续测距 */
    VL53L0X_SetDeviceMode(dev, DEMO_DEVICE_MODE);
    
    /* 设置测量预算时间(20ms) */
    VL53L0X_SetMeasurementTimingBudgetMicroSeconds(dev, DEMO_BUDGET_TIME);
}

/**
 * @brief       TOF传感器初始化
 * @param       无
 * @retval      无
 * @note        初始化硬件接口、通信接口，并配置设备参数
 */
void TOF_Init(void)
{
    atk_ms53l0_hw_init();                               /* 硬件接口初始化 */
    VL53L0X_comms_initialise(NULL, NULL);               /* 通信接口初始化 */
    demo_detect_device(&demo_dev, DEMO_DEV_IIC_ADDDR);  /* 检测设备并配置地址 */
    demo_config_device(&demo_dev);                      /* 配置设备参数 */
}

/**
 * @brief       运行TOF测距并返回测量结果
 * @param       无
 * @retval      测量得到的距离值(毫米)
 * @note        包含10ms超时处理和数据低通滤波
 */
uint16_t demo_run(void)
{
    uint8_t ret;
    VL53L0X_RangingMeasurementData_t data;
    uint16_t TOF_DATA_filter;
    uint32_t timeout = HAL_GetTick() + 10;  /* 10ms超时设置 */
    
    /* 启动设备测量 */
    VL53L0X_StartMeasurement(&demo_dev);
    
    /* 等待设备测量完成 */
    do {
        VL53L0X_GetMeasurementDataReady(&demo_dev, &ret);
        
        /* 超时检查 */
        if (HAL_GetTick() > timeout) {
            /* 超时处理：直接跳出循环 */
            break;
        }
    } while (ret != 1);
    
    /* 清除中断标志 */
    VL53L0X_ClearInterruptMask(&demo_dev, 0);
    
    /* 读取测量数据 */
    VL53L0X_GetRangingMeasurementData(&demo_dev, &data);
    
    /* 对测量数据进行低通滤波处理 */
    TOF_DATA_filter = Lowpassfilter(&TOF_Filter, data.RangeMilliMeter);
    
    /* 发送距离数据到串口(当前已注释掉) */
//    Send_Distance(TOF_DATA_filter);
    
    /* 返回滤波后的距离值 */
    return(TOF_DATA_filter);
}

/**
 * @brief       通过串口发送距离数据
 * @param       distance: 要发送的距离值(毫米)
 * @retval      无
 */
//void Send_Distance(uint16_t distance) {
//    char buffer[32];
//    int len = snprintf(buffer, sizeof(buffer), "Distance: %dmm\r\n", distance);
//    HAL_UART_Transmit(&huart1, (uint8_t *)buffer, len, 100);
//}
